/***************************************************************************
 创建者: 华磊
 开始时间: 2018.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2017.5.8 类的具体实现代码编写

 ***************************************************************************/
#include "canbusdevicedriver.h"
#include "controlcan.h"
#include <ctime>
#include <cstdlib>
#include "unistd.h"
#include <qdebug.h>
#include "messagelog.h"
#include "../EcatCore/ecatconfig.h"
#define D_RECEIVE_WAIT_COUNT_MAX 100
#define D_RECEIVE_WAIT_SLEEP_TIME 100

CanbusDeviceDriver::CanbusDeviceDriver(QString canConfigFile)
{
    isInitialOk=false;
    initialDevice();
}

int CanbusDeviceDriver::clearUserFrameBuffer()
{

    mutex.lock();
    userFrameBuffers1.clear();
    userFrameBuffers2.clear();
    mutex.unlock();
    return 1;
}



int CanbusDeviceDriver::sendSyncFrame(int deviceId, int canbusId)
{
    unsigned int frameId=0x80;
    QVector<unsigned char> dataList;
    dataList.clear();
    return sendData(deviceId,canbusId,frameId,dataList);
}

int CanbusDeviceDriver::manageNode(int deviceId, int canbusId,unsigned int nodeId, E_NODE_MANAGE commandIn)
{
    unsigned int frameId=0;
    QVector<unsigned char> dataList;
    dataList.resize(2);
    dataList[0]=commandIn;
    dataList[1]=nodeId;
    return sendData(deviceId,canbusId,frameId,dataList);
}



int CanbusDeviceDriver::sendPdoData(int deviceId, int canbusId, unsigned int nodeId,
                                    unsigned int pdoId, QVector<unsigned char> dataList)
{
    unsigned int frameId;
    switch(pdoId)
    {
    case 1:
    {
        frameId=0x200+nodeId;//0x180
        break;
    }
    case 2:
    {
        frameId=0x300+nodeId;//0x280
         break;
    }
    case 3:
    {
        frameId=0x400+nodeId;//0x380
         break;
    }
    case 4:
    {
        frameId=0x500+nodeId;//0x480
         break;
    }
    default:
        return -1;
    }

    return sendData(deviceId,canbusId,frameId,dataList);

}

int CanbusDeviceDriver::sendPdoData_toMaster(int deviceId, int canbusId,
                           unsigned int nodeId, unsigned int pdoId, QVector<unsigned char> dataList)
{
    unsigned int frameId;
    switch(pdoId)
    {
    case 1:
    {
        frameId=0x180+nodeId;//0x180
        break;
    }
    case 2:
    {
        frameId=0x280+nodeId;//0x280
         break;
    }
    case 3:
    {
        frameId=0x380+nodeId;//0x380
         break;
    }
    case 4:
    {
        frameId=0x480+nodeId;//0x480
         break;
    }
    default:
        return -1;
    }

    return sendData(deviceId,canbusId,frameId,dataList);
}

int CanbusDeviceDriver::sendNodeWatchDogFrame(int deviceId, int canbusId, unsigned int nodeId)
{
    //从站发送的心跳报文格式：　0x600+id,状态（１ｂｙｔｅ). 一般１秒产生１次。
    //主站发送节点保护报文格式：　0x700+id
    //主站响应节点保护报文格式：　0x700+id，状态（１ｂｙｔｅ).ｂｉｔ7交替１变０变１．
    //超过　节点保护时间（100c0010)ｘ节点保护系数(100d0008)　后从站报错（通讯中断模式60070010＝１）。

    //send sdo data
    unsigned int frameId=0x700+nodeId;
<<<<<<< HEAD
    int keyReturn = sendData_remote(deviceId, canbusId, frameId);
=======
    QVector<unsigned char> dataList;
    dataList.resize(1);
    int keyReturn = sendData(deviceId, canbusId, frameId, dataList);
>>>>>>> origin/master
//    qDebug()<<"sendNodeWatchDogFrame,deviceId="<<deviceId<<" canbusId="<<canbusId<<" frameId="<<frameId
//           <<"datalist"<<dataList;
    if(1!=keyReturn)
    {
        qDebug()<<"error, canbus send error!";
        return -1;
    }
    return 1;

}

int CanbusDeviceDriver::sendData(int deviceId, int canbusId, unsigned int frameId, QVector<unsigned char> dataList)
{
//    qDebug()<<"sendData "<<"deviceId="<<deviceId<<" canbusId="<<canbusId;
    //需要发送的帧，结构体设置
    VCI_CAN_OBJ send[1];
    send[0].ID=frameId;
    send[0].SendType=0;//0如果失败会重发，最长重发时间３Ｓ，１如果失败不会重发，建议用１．如果总线繁忙，会导致数据帧没有发出去。
    send[0].RemoteFlag=0;
    send[0].ExternFlag=0;
    send[0].DataLen=dataList.size();

    int i=0;
    for(i = 0; i < send[0].DataLen; i++)
    {
        send[0].Data[i] = dataList[i];
    }


    if(VCI_Transmit(VCI_USBCAN2, deviceId, canbusId, send, 1) == 1)
    {
//        printf("CAN1 TX ID:0x%08X",send[0].ID);
//        if(send[0].ExternFlag==0) printf(" Standard ");
//        if(send[0].ExternFlag==1) printf(" Extend   ");
//        if(send[0].RemoteFlag==0) printf(" Data   ");
//        if(send[0].RemoteFlag==1) printf(" Remote ");
//        printf("DLC:0x%02X",send[0].DataLen);
//        printf(" data:0x");

//        for(i=0;i<send[0].DataLen;i++)
//        {
//            printf(" %02X",send[0].Data[i]);
//        }

//        printf("\n");
//        send[0].ID+=1;
    }
    else
    {
        return -1;//从站没有ack应答，会导致重发，重发仍然失败，就会报错。
    }

    return 1;


}

int CanbusDeviceDriver::sendData_ext(int deviceId, int canbusId, unsigned int frameId, QVector<unsigned char> dataList)
{
    //    qDebug()<<"sendData_ext "<<"deviceId="<<deviceId<<" canbusId="<<canbusId;
        //需要发送的帧，结构体设置
        VCI_CAN_OBJ send[1];
        send[0].ID=frameId;
        send[0].SendType=0;//0如果失败会重发，最长重发时间３Ｓ，１如果失败不会重发，建议用１．
        send[0].RemoteFlag=0;
        send[0].ExternFlag=1;
        send[0].DataLen=dataList.size();

        int i=0;
        for(i = 0; i < send[0].DataLen; i++)
        {
            send[0].Data[i] = dataList[i];
        }


        if(VCI_Transmit(VCI_USBCAN2, deviceId, canbusId, send, 1) == 1)
        {
    //        printf("CAN1 TX ID:0x%08X",send[0].ID);
    //        if(send[0].ExternFlag==0) printf(" Standard ");
    //        if(send[0].ExternFlag==1) printf(" Extend   ");
    //        if(send[0].RemoteFlag==0) printf(" Data   ");
    //        if(send[0].RemoteFlag==1) printf(" Remote ");
    //        printf("DLC:0x%02X",send[0].DataLen);
    //        printf(" data:0x");

    //        for(i=0;i<send[0].DataLen;i++)
    //        {
    //            printf(" %02X",send[0].Data[i]);
    //        }

    //        printf("\n");
    //        send[0].ID+=1;
        }
        else
        {
            return -1;
        }

        return 1;
}

int CanbusDeviceDriver::sendData_remote(int deviceId, int canbusId, unsigned int frameId)
{
    //    qDebug()<<"sendData "<<"deviceId="<<deviceId<<" canbusId="<<canbusId;
        //需要发送的帧，结构体设置
        VCI_CAN_OBJ send[1];
        send[0].ID=frameId;
        send[0].SendType=0;//0如果失败会重发，最长重发时间３Ｓ，１如果失败不会重发，建议用１．如果总线繁忙，会导致数据帧没有发出去。
        send[0].RemoteFlag=1;
        send[0].ExternFlag=0;
        send[0].DataLen=0;



        if(VCI_Transmit(VCI_USBCAN2, deviceId, canbusId, send, 1) == 1)
        {
    //        printf("CAN1 TX ID:0x%08X",send[0].ID);
    //        if(send[0].ExternFlag==0) printf(" Standard ");
    //        if(send[0].ExternFlag==1) printf(" Extend   ");
    //        if(send[0].RemoteFlag==0) printf(" Data   ");
    //        if(send[0].RemoteFlag==1) printf(" Remote ");
    //        printf("DLC:0x%02X",send[0].DataLen);
    //        printf(" data:0x");

    //        for(i=0;i<send[0].DataLen;i++)
    //        {
    //            printf(" %02X",send[0].Data[i]);
    //        }

    //        printf("\n");
    //        send[0].ID+=1;
        }
        else
        {
            return -1;
        }

        return 1;
}

int CanbusDeviceDriver::receiveData(int deviceId, int canbusId,QVector<unsigned int> &frameIdOut,
                                    QVector<QVector<unsigned char> > &dataListReturn)
{
    int reclen=0;
    VCI_CAN_OBJ rec[3000];//接收缓存，设为3000为佳。
    if((reclen=VCI_Receive(VCI_USBCAN2,deviceId,canbusId,rec,3000,100))>0)//调用接收函数，如果有数据，进行数据处理显示。
    {
        dataListReturn.resize(reclen);
        frameIdOut.resize(reclen);
        for(int j=0;j<reclen;j++)
        {
            dataListReturn[j].resize(8);
            for(int i=0;i<8;i++)
            {
                dataListReturn[j][i]=rec[j].Data[i];
            }
            frameIdOut[j]=rec[j].ID;
//            printf("CAN%d RX ID:0x%08X", canbusId, rec[j].ID);//ID
//            if(rec[j].ExternFlag==0) printf(" Standard ");//帧格式：标准帧
//            if(rec[j].ExternFlag==1) printf(" Extend   ");//帧格式：扩展帧
//            if(rec[j].RemoteFlag==0) printf(" Data   ");//帧类型：数据帧
//            if(rec[j].RemoteFlag==1) printf(" Remote ");//帧类型：远程帧
//            printf("DLC:0x%02X",rec[j].DataLen);//帧长度
//            printf(" data:0x");	//数据
//            for(int i = 0; i < rec[j].DataLen; i++)
//            {
//                printf(" %02X", rec[j].Data[i]);
//            }
//            printf(" TimeStamp:0x%08X",rec[j].TimeStamp);//时间标识。
//            printf("\n");
        }
    }
    else
    {
        return -1;
    }

    return reclen;


}

int CanbusDeviceDriver::receiveData_toUserBuffer1(int deviceId, int canbusId)
{
   mutex.lock();
   int tmpKey=receiveData_toBuffer(deviceId,canbusId,userFrameBuffers1);
   mutex.unlock();
   return tmpKey;

}

int CanbusDeviceDriver::receiveData_toUserBuffer2(int deviceId, int canbusId)
{
   mutex.lock();
   int tmpKey=receiveData_toBuffer(deviceId,canbusId,userFrameBuffers2);
   mutex.unlock();
   return tmpKey;

}

int CanbusDeviceDriver::receiveData_toBuffer(int deviceId, int canbusId, QQueue<FrameData> &bufferOut)
{

    QVector<unsigned int> frameIdOut;
    QVector<QVector<unsigned char> > dataListReturn;
    int length=receiveData(deviceId,canbusId,frameIdOut,dataListReturn);
    if(length>0)
    {
        FrameData tmpData;
        for(int i=0;i<frameIdOut.size();i++)
        {
            tmpData.frameId=frameIdOut[i];
            tmpData.data=dataListReturn[i];
            bufferOut.enqueue(tmpData);
        }
//        qDebug()<<"1111111111111111111111111111receiveData length="<<length;
//        qDebug()<<"frameId="<<frameIdOut;
//        qDebug()<<"dataListReturn="<<dataListReturn;

    }

    return 1;
}

int CanbusDeviceDriver::getPdoData_ch1( QVector<unsigned int> &nodeIdOut,QVector<unsigned int> &pdoIdOut,
                                        QVector<QVector<unsigned char> > &dataListOut)
{
    return getPdoData(0,nodeIdOut,pdoIdOut,dataListOut);
}

int CanbusDeviceDriver::getPdoMasterData_ch1(QVector<unsigned int> &nodeIdOut,
             QVector<unsigned int> &pdoIdOut, QVector<QVector<unsigned char> > &dataListOut)
{
    return getPdoMasterData(0,nodeIdOut,pdoIdOut,dataListOut);
}

int CanbusDeviceDriver::getPdoData_ch2(QVector<unsigned int> &nodeIdOut, QVector<unsigned int> &pdoIdOut, QVector<QVector<unsigned char> > &dataListOut)
{
        return getPdoData(1,nodeIdOut,pdoIdOut,dataListOut);
}

int CanbusDeviceDriver::getUserBuffer(QQueue<FrameData> &userFrameBuffersOut)
{
    mutex.lock();
    userFrameBuffersOut=userFrameBuffers1;
    mutex.unlock();
    return 1;
}

int CanbusDeviceDriver::getUserBuffer(int busIdIn, QQueue<FrameData> &userFrameBuffersOut)
{
    mutex.lock();
    switch(busIdIn)
    {
    case 0:
    {
        userFrameBuffersOut=userFrameBuffers1;
        break;
    }
    case 1:
    {
        userFrameBuffersOut=userFrameBuffers2;
        break;
    }
    }
    mutex.unlock();
    return 1;
}



int CanbusDeviceDriver::getReceiveNum(int deviceId, int canbusId)
{
    return VCI_GetReceiveNum(VCI_USBCAN2,deviceId,canbusId);
}

int CanbusDeviceDriver::closeDevice()
{
    realTimeSleep(100000);//延时100ms。
    VCI_ResetCAN(VCI_USBCAN2, 0, 0);//复位CAN1通道。
    realTimeSleep(100000);//延时100ms。
    VCI_ResetCAN(VCI_USBCAN2, 0, 1);//复位CAN2通道。
    realTimeSleep(100000);//延时100ms。
    VCI_CloseDevice(VCI_USBCAN2,0);//关闭设备。
    //除收发函数外，其它的函数调用前后，最好加个毫秒级的延时，即不影响程序的运行，又可以让USBCAN设备有充分的时间处理指令。
    return 1;
}

bool CanbusDeviceDriver::isInitialOkStatus()
{
    return isInitialOk;
}

#define D_SDO_WAIT_MAX 7
int CanbusDeviceDriver::sdoUpload(int deviceIdIn, int canbusIdIn,int node,
                                  int rObIndex, int rObSubIndex, int64_t &rData, int &rDataLen)
{
    //send sdo data
    unsigned int frameId=0x600+node;
    QVector<unsigned char> dataList;
    dataList.resize(8);
    dataList[0]=0x40;
    dataList[1]=rObIndex  & 0xff;
    dataList[2]=rObIndex>>8;
    dataList[3]=rObSubIndex;
    mutex.lock();
    int keyReturn = sendData(deviceIdIn, canbusIdIn, frameId, dataList);
    if(1!=keyReturn)
    {
        qDebug()<<"error, canbus send error!";
//        addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1915, -1 );
        mutex.unlock();
        return -1;

    }
    mutex.unlock();

    int tmpReturn;
    for(int i=0;i<D_SDO_WAIT_MAX;i++)
    {
        tmpReturn=receiveUploadSdoDataResponse(deviceIdIn, canbusIdIn, node,dataList,rData,rDataLen);

        if(1==tmpReturn)
        {
            return 1;
        }
    }
    return tmpReturn;


}

int CanbusDeviceDriver::sdoDownload(int deviceIdIn, int canbusIdIn,int node,
                                    int wObIndex, int wObSubIndex, int64_t wData, int wDataLen)
{
    //send sdo data
    unsigned int frameId=0x600+node;
    QVector<unsigned char> dataList;
    dataList.resize(8);
    dataList[1]=wObIndex  & 0xff;
    dataList[2]=wObIndex>>8;
    dataList[3]=wObSubIndex;
    if(1==wDataLen)
    {
        dataList[0]=0x2f;

        dataList[4]=wData;
        dataList[5]=0;
        dataList[6]=0;
        dataList[7]=0;
    }
    else if(2==wDataLen)
    {
        dataList[0]=0x2b;

        dataList[4]= wData& 0xff;
        dataList[5]= wData  >> 8 ;
        dataList[6]=0;
        dataList[7]=0;
    }
    else if(4==wDataLen)
    {
        dataList[0]=0x23;
//        dataList[0]=0x22;源本磁传感器

        dataList[7]= wData >> 24 ;
        dataList[6]=(wData& 0xff0000)>>16;
        dataList[5]=(wData& 0xff00)>>8;;
        dataList[4]=wData& 0xff;
    }
    else
    {
        qDebug()<<"error, sdoDownload wDataLen error!";
        return -1;
    }


    mutex.lock();
    int keyReturn = sendData(deviceIdIn, canbusIdIn, frameId, dataList);
    if(1!=keyReturn)
    {
        qDebug()<<"error, canbus send error!";
//        addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1915, -1 );
        mutex.unlock();
        return -2;

    }
    mutex.unlock();

    int tmpReturn;
    for(int i=0;i<D_SDO_WAIT_MAX;i++)
    {
        //用户层使用该函数时，可能被pdo接收时清空了数据缓冲区，导致认为失败。
        tmpReturn=receiveDownloadSdoDataResponse(deviceIdIn, canbusIdIn, node,dataList);

        if(1==tmpReturn)
        {
            return 1;
        }
    }
    return tmpReturn;

}

int CanbusDeviceDriver::sdoUpload_noCareFeedback(int deviceIdIn, int canbusIdIn, int node, int rObIndex,
                                                 int rObSubIndex)
{
    //send sdo data
    unsigned int frameId=0x600+node;
    QVector<unsigned char> dataList;
    dataList.resize(8);
    dataList[0]=0x40;
    dataList[1]=rObIndex  & 0xff;
    dataList[2]=rObIndex>>8;
    dataList[3]=rObSubIndex;
    int keyReturn = sendData(deviceIdIn, canbusIdIn, frameId, dataList);
    if(1!=keyReturn)
    {
        qDebug()<<"error, canbus send error!";
//        addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1915, -1 );
        return -1;

    }
    return 1;
}

int CanbusDeviceDriver::sdoDownload_noCareFeedback(int deviceIdIn, int canbusIdIn, int node, int wObIndex,
                                                   int wObSubIndex, int64_t wData, int wDataLen)
{
    //send sdo data
    unsigned int frameId=0x600+node;
    QVector<unsigned char> dataList;
    dataList.resize(8);
    dataList[1]=wObIndex  & 0xff;
    dataList[2]=wObIndex>>8;
    dataList[3]=wObSubIndex;
    if(1==wDataLen)
    {
        dataList[0]=0x2f;

        dataList[4]=wData;
        dataList[5]=0;
        dataList[6]=0;
        dataList[7]=0;
    }
    else if(2==wDataLen)
    {
        dataList[0]=0x2b;

        dataList[4]= wData& 0xff;
        dataList[5]= wData  >> 8 ;
        dataList[6]=0;
        dataList[7]=0;
    }
    else if(4==wDataLen)
    {
        dataList[0]=0x23;

        dataList[7]= wData >> 24 ;
        dataList[6]=(wData& 0xff0000)>>16;
        dataList[5]=(wData& 0xff00)>>8;;
        dataList[4]=wData& 0xff;
    }
    else
    {
        qDebug()<<"error, sdoDownload wDataLen error!";
        return -1;
    }



    int keyReturn = sendData(deviceIdIn, canbusIdIn, frameId, dataList);
    if(1!=keyReturn)
    {
        qDebug()<<"error, canbus send error!";
//        addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1915, -1 );
        return -2;

    }
    return 1;
}

int CanbusDeviceDriver::sdoUpload_receive(int deviceIdIn, int canbusIdIn,int node
                                          ,int rObIndex
                                          ,int rObSubIndex
                                          ,LONG64 &rData
                                          ,int &rDataLen)
{
    //sdo data
    QVector<unsigned char> dataList;
    dataList.resize(8);
    dataList[0]=0x40;
    dataList[1]=rObIndex  & 0xff;
    dataList[2]=rObIndex>>8;
    dataList[3]=rObSubIndex;

    //process data
    for(int i=0;i<userFrameBuffers1.size();i++)
    {
        if(userFrameBuffers1[i].frameId==0x580+node)
        {
            if(userFrameBuffers1[i].data[1]==dataList[1] && userFrameBuffers1[i].data[2]==dataList[2]
                    && userFrameBuffers1[i].data[3]==dataList[3])
            {
                if(userFrameBuffers1[i].data[0]==0x4f)
                {
                    rDataLen=1;
                    rData=userFrameBuffers1[i].data[4];
                }
                else if(userFrameBuffers1[i].data[0]==0x4b)
                {
                    rDataLen=2;
                    rData=userFrameBuffers1[i].data[4];
                    rData+=userFrameBuffers1[i].data[5]<<8;
                }
                else if(userFrameBuffers1[i].data[0]==0x43)
                {
                    rDataLen=4;
                    rData=userFrameBuffers1[i].data[4];
                    rData+=userFrameBuffers1[i].data[5]<<8;
                    rData+=userFrameBuffers1[i].data[6]<<16;
                    rData+=userFrameBuffers1[i].data[7]<<24;
                }
                else if(userFrameBuffers1[i].data[0]==0x80)
                {
                    rDataLen=0;
                    qDebug()<<"error, canbus receive error,dataListOut[i][0]==0x80,sdoupload_buffer!"
                           <<userFrameBuffers1[i].data;
//                    addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1915, -1 );
                    return -3;
                }
                else
                {
                    qDebug()<<"error, can receive unknow data, length!";
                    return -4;
                }
                return 1;
            }
            else
            {
                continue;
            }

        }
        else
        {
//            qDebug()<<"warning, can receive unknow frameId frame!";
            continue;
        }
    }
    return -6;
}

int CanbusDeviceDriver::getPdoData(int channelId,QVector<unsigned int> &nodeIdOut,
                    QVector<unsigned int> &pdoIdOut, QVector<QVector<unsigned char> > &dataListOut)
{
    nodeIdOut.clear();
    pdoIdOut.clear();
    dataListOut.clear();
    QQueue<FrameData> tmpFrames;
    if(channelId<0 || channelId>2)
    {
        return -1;
    }
    else if(0==channelId)
    {
        mutex.lock();
        tmpFrames=userFrameBuffers1;
        mutex.unlock();
    }
    else if(1==channelId)
    {
        mutex.lock();
        tmpFrames=userFrameBuffers2;
        mutex.unlock();
    }

//     qDebug()<<"getPdoData----------------------------------";

    //process data
    for(int i=0;i<tmpFrames.size();i++)
    {
//        qDebug()<<"userFrameBuffers[i].frameId="<<userFrameBuffers[i].frameId;

        if(tmpFrames[i].frameId>=0x180 && tmpFrames[i].frameId<=0x1ff)
        {
            //kinco frameId为什么会多了１　？
            nodeIdOut.append(tmpFrames[i].frameId-0x180);
            pdoIdOut.append(1);
            dataListOut.append(tmpFrames[i].data);

        }
        else if(tmpFrames[i].frameId>=0x280 && tmpFrames[i].frameId<=0x2ff)
        {
            nodeIdOut.append(tmpFrames[i].frameId-0x280);
            pdoIdOut.append(2);
            dataListOut.append(tmpFrames[i].data);

        }
        else if(tmpFrames[i].frameId>=0x380 && tmpFrames[i].frameId<=0x3ff)
        {
            nodeIdOut.append(tmpFrames[i].frameId-0x380);
            pdoIdOut.append(3);
            dataListOut.append(tmpFrames[i].data);

        }
        else if(tmpFrames[i].frameId>=0x480 && tmpFrames[i].frameId<=0x4ff)
        {
            nodeIdOut.append(tmpFrames[i].frameId-0x480);
            pdoIdOut.append(4);
            dataListOut.append(tmpFrames[i].data);

        }
        else
        {
//            qDebug()<<"pdo process warning, can receive unknow frameId; frameId="<<userFrameBuffers[i].frameId;
            continue;
        }
    }

    return 1;
}

int CanbusDeviceDriver::getPdoMasterData(int channelId, QVector<unsigned int> &nodeIdOut,
              QVector<unsigned int> &pdoIdOut, QVector<QVector<unsigned char> > &dataListOut)
{
    nodeIdOut.clear();
    pdoIdOut.clear();
    dataListOut.clear();
    QQueue<FrameData> tmpFrames;
    if(channelId<0 || channelId>2)
    {
        return -1;
    }
    else if(0==channelId)
    {
        mutex.lock();
        tmpFrames=userFrameBuffers1;
        mutex.unlock();
    }
    else if(1==channelId)
    {
        mutex.lock();
        tmpFrames=userFrameBuffers2;
        mutex.unlock();
    }

//     qDebug()<<"getPdoData----------------------------------";

    //process data
    for(int i=0;i<tmpFrames.size();i++)
    {
//        qDebug()<<"userFrameBuffers[i].frameId="<<userFrameBuffers[i].frameId;

        if(tmpFrames[i].frameId>=0x200 && tmpFrames[i].frameId<=0x27f)
        {
            //kinco frameId为什么会多了１　？
            nodeIdOut.append(tmpFrames[i].frameId-0x200);
            pdoIdOut.append(1);
            dataListOut.append(tmpFrames[i].data);

        }
        else if(tmpFrames[i].frameId>=0x300 && tmpFrames[i].frameId<=0x37f)
        {
            nodeIdOut.append(tmpFrames[i].frameId-0x300);
            pdoIdOut.append(2);
            dataListOut.append(tmpFrames[i].data);

        }
        else if(tmpFrames[i].frameId>=0x400 && tmpFrames[i].frameId<=0x47f)
        {
            nodeIdOut.append(tmpFrames[i].frameId-0x400);
            pdoIdOut.append(3);
            dataListOut.append(tmpFrames[i].data);

        }
        else if(tmpFrames[i].frameId>=0x500 && tmpFrames[i].frameId<=0x57f)
        {
            nodeIdOut.append(tmpFrames[i].frameId-0x500);
            pdoIdOut.append(4);
            dataListOut.append(tmpFrames[i].data);

        }
        else
        {
//            qDebug()<<"pdo process warning, can receive unknow frameId; frameId="<<userFrameBuffers[i].frameId;
            continue;
        }
    }

    return 1;
}

int CanbusDeviceDriver::receiveUploadSdoDataResponse(int deviceIdIn, int canbusIdIn,int node,
                                       QVector<unsigned char> dataList,int64_t &rData, int &rDataLen)
{
    int keyReturn;
     mutex.lock();
    //receive sdo data
    QVector<unsigned int> frameIdOut;
    QVector<QVector<unsigned char> > dataListOut;
    int tryCount=0;
    while(tryCount<D_RECEIVE_WAIT_COUNT_MAX)
    {
        keyReturn = receiveData(deviceIdIn, canbusIdIn, frameIdOut, dataListOut);
        if(keyReturn>0)
        {
            break;
        }
        tryCount++;
        realTimeSleep(D_RECEIVE_WAIT_SLEEP_TIME);

    }
    mutex.unlock();
    if(keyReturn<=0)
    {
        qDebug()<<"error, canbus receive error,,timeout and no data received sdo upload!";
//        addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1915, -1 );
        return -2;
    }

    //保存接收数据
    mutex.lock();
    FrameData tmpData;
    for(int i=0;i<frameIdOut.size();i++)
    {
        tmpData.frameId=frameIdOut[i];
        tmpData.data=dataListOut[i];
        userFrameBuffers1.enqueue(tmpData);
    }
    mutex.unlock();

    //process data
    for(int i=0;i<frameIdOut.size();i++)
    {
        if(frameIdOut[i]==0x580+node)
        {
            if(dataListOut[i][1]==dataList[1] && dataListOut[i][2]==dataList[2]
                    && dataListOut[i][3]==dataList[3])
            {
                if(dataListOut[i][0]==0x4f)
                {
                    rDataLen=1;
                    rData=dataListOut[i][4];
                }
                else if(dataListOut[i][0]==0x4b)
                {
                    rDataLen=2;
                    rData=dataListOut[i][4];
                    rData+=dataListOut[i][5]<<8;
                }
                else if(dataListOut[i][0]==0x43)
                {
                    rDataLen=4;
                    rData=dataListOut[i][4];
                    rData+=dataListOut[i][5]<<8;
                    rData+=dataListOut[i][6]<<16;
                    rData+=dataListOut[i][7]<<24;
                }
                else if(dataListOut[i][0]==0x42)
                {
                    rDataLen=4;
                    rData=dataListOut[i][4];
                    rData+=dataListOut[i][5]<<8;
                    rData+=dataListOut[i][6]<<16;
                    rData+=dataListOut[i][7]<<24;
                    qDebug()<<"warn, can sdo receive data,flag=0x42 dataListOut[i]="<<dataListOut[i];
                }
                else if(dataListOut[i][0]==0x80)
                {
                    rDataLen=0;
                    qDebug()<<"error, canbus receive error,dataListOut[i][0]==0x80,sdo upload !"
                           <<dataListOut[i];
//                    addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1915, -1 );
                    return -3;
                }
                else
                {
                    //amc驱动器返回0x42
                    qDebug()<<"error, can sdo receive unknow data, dataListOut[i]="<<dataListOut[i];
                    return -4;
                }
                return 1;
            }
            else
            {
                qDebug()<<"error, can receive unknow data, , frameId ok, sdo not ok,sdo upload!";
                return -5;
            }

        }
        else
        {
//            qDebug()<<"warning, can receive unknow frameId frame!";
            continue;
        }
    }
    return -6;
}

int CanbusDeviceDriver::receiveDownloadSdoDataResponse(int deviceIdIn, int canbusIdIn, int node,
                                                       QVector<unsigned char> dataList)
{
    mutex.lock();
    int keyReturn;
    //receive sdo data
    QVector<unsigned int> frameIdOut;
    QVector<QVector<unsigned char> > dataListOut;
    int tryCount=0;
    while(tryCount<D_RECEIVE_WAIT_COUNT_MAX)
    {
        keyReturn = receiveData(deviceIdIn, canbusIdIn, frameIdOut, dataListOut);
        if(keyReturn>0)
        {
            break;
        }
        tryCount++;
        realTimeSleep(D_RECEIVE_WAIT_SLEEP_TIME);

    }
    mutex.unlock();
    if(keyReturn<=0)
    {
        qDebug()<<"error, canbus receive error, timeout and no received sdodownlaod!";
//        addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1915, -1 );
        return -3;
    }

    //保存接收数据
    mutex.lock();
    FrameData tmpData;
    for(int i=0;i<frameIdOut.size();i++)
    {
        tmpData.frameId=frameIdOut[i];
        tmpData.data=dataListOut[i];
        userFrameBuffers1.enqueue(tmpData);
    }
    mutex.unlock();

    //process data
    for(int i=0;i<frameIdOut.size();i++)
    {
        if(frameIdOut[i]==0x580+node)
        {
            if(dataListOut[i][1]==dataList[1] && dataListOut[i][2]==dataList[2]
                    && dataListOut[i][3]==dataList[3])
            {
                if(dataListOut[i][0]==0x60)
                {
                    return 1;
                }
                else if(dataListOut[i][0]==0x80)
                {
                    qDebug()<<"error, canbus receive error,dataListOut[i][0]==0x80 ,sdo download!"
                           <<dataListOut[i];
//                    addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1915, -1 );
                    return -4;
                }
                else
                {
                    qDebug()<<"error, can receive unknow data, length!";
                    return -5;
                }

            }
            else
            {
                qDebug()<<"error, can receive unknow data, frameId ok, sdo not ok,sdo downlaod!";
                return -6;
            }

        }
        else
        {
//            qDebug()<<"warning, can receive unknow frameId frame!";
            continue;
        }
    }
    return -7;
}


void CanbusDeviceDriver::realTimeSleep(long long umicoroSecond)
{
    struct timespec time;
    clock_gettime(CLOCK_REALTIME,&time);
    time.tv_nsec += umicoroSecond * 1000;
    while(time.tv_nsec >= NSEC_PER_SEC)
    {
        time.tv_nsec -= NSEC_PER_SEC;
        ++time.tv_sec;
    }
    clock_nanosleep(CLOCK_REALTIME,TIMER_ABSTIME,&time,NULL);

}

int CanbusDeviceDriver::initialDevice()
{
        printf("CanbusDeviceDriver::initialDevice >>this is hello !\r\n");//指示程序已运行
        closeDevice();
        if(VCI_OpenDevice(VCI_USBCAN2,0,0)==1)//打开设备
        {
            printf(">>open deivce success!\n");//打开设备成功
        }
        else
        {

            printf(">>open deivce error!\n");
            addMsg(ENUM_MSG_ERROR,"CanbusDeviceDriver","CanbusDeviceDriver",3800,-1);
            return -1;
        }
        VCI_BOARD_INFO pInfo;//用来获取设备信息。
        if(VCI_ReadBoardInfo(VCI_USBCAN2,0,&pInfo)==1)//读取设备序列号、版本等信息。
        {
                    printf(">>Get VCI_ReadBoardInfo success!\n");

            //printf(" %08X", pInfo.hw_Version);printf("\n");
            //printf(" %08X", pInfo.fw_Version);printf("\n");
            //printf(" %08X", pInfo.dr_Version);printf("\n");
            //printf(" %08X", pInfo.in_Version);printf("\n");
            //printf(" %08X", pInfo.irq_Num);printf("\n");
            //printf(" %08X", pInfo.can_Num);printf("\n");
//            printf(">>Serial_Num:%c", pInfo.str_Serial_Num[0]);
//            printf("%c", pInfo.str_Serial_Num[1]);
//            printf("%c", pInfo.str_Serial_Num[2]);
//            printf("%c", pInfo.str_Serial_Num[3]);
//            printf("%c", pInfo.str_Serial_Num[4]);
//            printf("%c", pInfo.str_Serial_Num[5]);
//            printf("%c", pInfo.str_Serial_Num[6]);
//            printf("%c", pInfo.str_Serial_Num[7]);
//            printf("%c", pInfo.str_Serial_Num[8]);
//            printf("%c", pInfo.str_Serial_Num[9]);
//            printf("%c", pInfo.str_Serial_Num[10]);
//            printf("%c", pInfo.str_Serial_Num[11]);
//            printf("%c", pInfo.str_Serial_Num[12]);
//            printf("%c", pInfo.str_Serial_Num[13]);
//            printf("%c", pInfo.str_Serial_Num[14]);
//            printf("%c", pInfo.str_Serial_Num[15]);
//            printf("%c", pInfo.str_Serial_Num[16]);
//            printf("%c", pInfo.str_Serial_Num[17]);
//            printf("%c", pInfo.str_Serial_Num[18]);
//            printf("%c", pInfo.str_Serial_Num[19]);printf("\n");

//            printf(">>hw_Type:%c", pInfo.str_hw_Type[0]);
//            printf("%c", pInfo.str_hw_Type[1]);
//            printf("%c", pInfo.str_hw_Type[2]);
//            printf("%c", pInfo.str_hw_Type[3]);
//            printf("%c", pInfo.str_hw_Type[4]);
//            printf("%c", pInfo.str_hw_Type[5]);
//            printf("%c", pInfo.str_hw_Type[6]);
//            printf("%c", pInfo.str_hw_Type[7]);
//            printf("%c", pInfo.str_hw_Type[8]);
//            printf("%c", pInfo.str_hw_Type[9]);printf("\n");
        }
        else
        {
            printf(">>Get VCI_ReadBoardInfo error!\n");
            addMsg(ENUM_MSG_ERROR,"CanbusDeviceDriver","CanbusDeviceDriver",3801,-1);
            return -1;
        }

        //初始化参数，严格参数二次开发函数库说明书。
        VCI_INIT_CONFIG config;
        config.AccCode=0;
        config.AccMask=0xFFFFFFFF;
        config.Filter=1;//接收所有帧
        /*波特率10 Kbps  0x31  0x1C*/
        /*波特率50 Kbps  0x09  0x1C*/
        /*波特率125 Kbps  0x03  0x1C*/
        config.Timing0=0x01;/*波特率125 Kbps  0x03  0x1C*/ /*波特率250 Kbps  0x01  0x1C*/
        config.Timing1=0x1C;
        config.Mode=0;//正常模式

        VCI_INIT_CONFIG config2;
        config2.AccCode=0;
        config2.AccMask=0xFFFFFFFF;
        config2.Filter=1;//接收所有帧
        config2.Timing0=0x01;/*波特率125 Kbps  0x03  0x1C*/ /*波特率250 Kbps  0x01  0x1C*/
        config2.Timing1=0x1C;
        config2.Mode=0;//正常模式

        if(VCI_InitCAN(VCI_USBCAN2,0,0,&config)!=1)
        {
            printf(">>Init CAN1 error\n");
            VCI_CloseDevice(VCI_USBCAN2,0);
            addMsg(ENUM_MSG_ERROR,"CanbusDeviceDriver","CanbusDeviceDriver",3802,-1);
            return -1;
        }

        if(VCI_StartCAN(VCI_USBCAN2,0,0)!=1)
        {
            printf(">>Start CAN1 error\n");
            VCI_CloseDevice(VCI_USBCAN2,0);
            addMsg(ENUM_MSG_ERROR,"CanbusDeviceDriver","CanbusDeviceDriver",3803,-1);
            return -1;

        }

        if(VCI_InitCAN(VCI_USBCAN2,0,1,&config2)!=1)
        {
            printf(">>Init can2 error\n");
            VCI_CloseDevice(VCI_USBCAN2,0);
            addMsg(ENUM_MSG_ERROR,"CanbusDeviceDriver","CanbusDeviceDriver",3804,-1);
            return -1;

        }
        if(VCI_StartCAN(VCI_USBCAN2,0,1)!=1)
        {
            printf(">>Start can2 error\n");
            VCI_CloseDevice(VCI_USBCAN2,0);
            addMsg(ENUM_MSG_ERROR,"CanbusDeviceDriver","CanbusDeviceDriver",3805,-1);
            return -1;

        }
        printf(">>Init can2 sucess\n");

        isInitialOk=true;

}


void CanbusDeviceDriver::addMsg(int messageLevel, string componentName, string messageType,
                           int messageCode, int robotId,
                           int parameter1, int parameter2, int parameter3, int parameter4)
{
    Message tmpMsg;
    tmpMsg.MessageLevel = messageLevel;
    tmpMsg.componentClassName = componentName;
    tmpMsg.messageType = messageType;
    tmpMsg.robotId = robotId;
    tmpMsg.messageCode = messageCode;



    QString infomationStr;

    switch(messageCode)
    {
    case 3805:
    {
        infomationStr =  QObject::tr("CAN适配器2通道启用失败！");
        break;
    }
    case 3804:
    {
        infomationStr =  QObject::tr("CAN适配器2通道初始化配置失败！");
        break;
    }
    case 3803:
    {
        infomationStr =  QObject::tr("CAN适配器１通道启用失败！");
        break;
    }
    case 3802:
    {
        infomationStr =  QObject::tr("CAN适配器１通道初始化配置失败！");
        break;
    }
    case 3801:
    {
        infomationStr =  QObject::tr("CAN适配器版本信息获取失败！");
        break;
    }
    case 3800:
    {
        infomationStr =  QObject::tr("CAN适配器没找到！");
        break;
    }


        break;
    }


    tmpMsg.MessageInformation = infomationStr.toStdString();

    MessageLog::getInstance()->addMessage(tmpMsg);
}
